/*
 *  camshift.c
 *  libCV
 *
 *  Created by Gregory Koch on 17/01/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#include "camshift.h"
#include "ccvt.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>


camshift_target_t init_camshift_target()
{
	camshift_target_t target;
	
	target.q = init_hist();
	
	target.zone = init_zone(init_pos(0, 0), init_size(0, 0));
    
	target.lock = 0;
    
	return target;
}

void calibrate_camshift_target(camshift_target_t *target, image_t img, zone_t zone)
{
 	if(img.type != IMG_BGR24 && img.type != IMG_RGB24)
	{
		perror("libCV : camshift works only with rgb and bgr images");
		exit(EXIT_FAILURE);
	}
	
	if(!verif_zone(img, zone))
	{
		printf("libCV : zone out of image");
		exit(EXIT_FAILURE);
	}
	
	target->q = calc_weighted_nhist(img, zone);
	
	target->zone = zone;
    
	target->lock = 1;
	
	target->img_size = img.size;   
}


int follow_camshift_target(camshift_target_t *target, image_t img)
{
	float *wi;
	int i, x, y, pos, xc, yc, stop, size;
	hsv_pixel_t hsv;
	double M00, M01, M10;
	zone_t py1_zone, search_zone;
    hist_t hist;
    
    if(img.type != IMG_BGR24 && img.type != IMG_RGB24)
	{
		perror("libCV : camshift works only with rgb and bgr images");
		exit(EXIT_FAILURE);
	}

    if(target->img_size.height != img.size.height || 
       target->img_size.width != img.size.width)
    {
        perror("libCV : camshift wrong image size");
		exit(EXIT_FAILURE);
    }
	
	search_zone = target->zone;
	resize_zone(&search_zone, init_size(search_zone.size.width * SEARCH_ZONE_SCALE, 
                                        search_zone.size.height * SEARCH_ZONE_SCALE), img);
	
	wi = malloc(sizeof(float)*search_zone.size.height*search_zone.size.width);
	if(wi == NULL)
	{
		perror("libCV : malloc error");
		exit(EXIT_FAILURE);
	}
	
	stop = 0;
	i = 0;
	while(i<CAMSHIFT_MAX_IT && !stop)
	{
		
		M00 = 0;
		M01 = 0;
		M10 = 0;
		
		
		for(y=0; y<search_zone.size.height; y++)
		{
			for(x=0; x<search_zone.size.width; x++)
			{
				pos = img.stride * (search_zone.pos.y + y)
				+ (search_zone.pos.x + x) * 3;
				
				if(img.type == IMG_BGR24)
				{
					hsv = ccvt_rgb24_hsv(img.data[pos+2], img.data[pos+1], img.data[pos]);
                    
				}
				else
				{
					hsv = ccvt_rgb24_hsv(img.data[pos], img.data[pos+1], img.data[pos+2]);	
				}
				
				wi[y*search_zone.size.width + x] = target->q.data[hsv.hue];
				
				M00 += wi[y*search_zone.size.width + x];
				M01 += y * wi[y*search_zone.size.width + x];
				M10 += x * wi[y*search_zone.size.width + x];
				
			}
		}
		
		if(!M00)
		{
			free(wi);
			target->lock = 0;
            target->zone = init_zone(init_pos(0, 0), init_size(img.size.width, img.size.height));
			return 0;
		}
		
		xc = M10/M00;
		yc = M01/M00;
		
		py1_zone = search_zone;
		center_zone(&py1_zone, init_pos(xc, yc), img);
		
		if(kernel_dist(search_zone, init_pos(xc, yc)) > CAMSHIFT_DIST_STOP)
		{
			stop = 1;
			search_zone = py1_zone;
		}
		else
		{
			search_zone = py1_zone;
		}
        
		i++;
	}
	
	size = 2*sqrt(M00);
	
	if(size < TARGET_SIZE_MIN)
	{
		target->lock = 0;
        target->zone = init_zone(init_pos(0, 0), init_size(img.size.width, img.size.height));
        return 0;
	}
	else if(size > TARGET_SIZE_MAX)
	{
		size = TARGET_SIZE_MAX;
	}
	
	resize_zone(&search_zone, init_size(size, size), img);
    
    if(target->lock)
    {
        target->zone = search_zone;
    }
    else
    {
        hist = calc_weighted_nhist(img, search_zone);
        
        if(correl_hist(target->q, hist) > CORREL_TARGET)
        {
            target->lock = 1;
            target->zone = search_zone;
        }
    }
    
    
	free(wi);
	
	return 1;
}


